%Main movement function for moving robot a given distance.
function returnedDistance = NXTMove(distance)
power = 50;
frictionFactor = 1.1;
%IS THIS INITIALISING STUFF STILL NEEDED????
% COM_CloseNXT all
% h = COM_OpenNXT();
% COM_SetDefaultNXT(h);
OpenSwitch(SENSOR_4)
%moves robot forward if needs to reverse.
distance2 = distance;
if distance < 0
    distance = -distance;
    power = -power;
end
%translates distance into correct amount of rotation of motors
tacholimit = round(distance/(4.4*pi)*360*frictionFactor);
%sends information to motors to both move in same direction.

mA = NXTMotor('A', 'Power', power, 'TachoLimit',tacholimit);
mB = NXTMotor('C', 'Power', power, 'TachoLimit',tacholimit);
Z = mA.ReadFromNXT.TachoCount;
Z2 = 100;
mA.SendToNXT();
mB.SendToNXT();
x = GetSwitch(SENSOR_4);
while (((abs(Z) < 10) | (abs(Z2) < 10)) | (Z ~= Z2)) & (x == 0)
    Z2 = Z;
    Z = mA.ReadFromNXT.TachoCount;
    pause(0.01)
    x = GetSwitch(SENSOR_4);
end

mA.Stop('brake')
mB.Stop('brake')
returnedDistance = mA.ReadFromNXT.TachoCount*distance2/tacholimit;